// Created by zengyuhao on 25-8-27.
//
#include "robot_manager.h"
#include "utilities/convert_data_type.hpp"

void RobotManager::propertiesCartesianPlannerCallback(const zoros::msg::PlannerProperties::SharedPtr msg)
{
    if (msg->velocities.size() == 3 && msg->accelerations.size() == 3 && msg->jerks.size() == 3)
    {
        ZLOGI("[Properties Cartesian Planner] Received command: \n"
              "max_velocities: [%f, %f, %f] \n"
              "max_accelerations: [%f, %f, %f] \n"
              "max_jerks: [%f, %f, %f] \n",
              msg->velocities[0], msg->velocities[1], msg->velocities[2], msg->accelerations[0], msg->accelerations[1],
              msg->accelerations[2], msg->jerks[0], msg->jerks[1], msg->jerks[2]);
    }
    else
    {
        ZLOGW("[Properties Cartesian Planner] Size Mismatch");
    }
    cartesian_planner_properties_.max_velocity = Vector2Eigen(msg->velocities);
    cartesian_planner_properties_.max_acceleration = Vector2Eigen(msg->accelerations);
    cartesian_planner_properties_.max_jerk = Vector2Eigen(msg->jerks);
}